#include "myinclude.h"
#include "Cmouse.h"
//extern mouseOCV mou,c,cccseStruct;
//void initMouse(Mat, cv::Size, UNIT);
//void onMouseCallback(int32_t event, int32_t x, int32_t y, int32_t flag, void * param);
#define MOUSE3D

//main Loop

int main(int argc, char **argv) {

    if (argc > 2) {
        std::cout << "Only the path of an image can be passed in arg" << std::endl;
        return -1;
    }

    Camera* zed;

    if (argc == 1) zed = new Camera(HD720);
    else zed = new Camera(argv[1]);

    // init computation mode of the zed
    InitParams parameters;
    parameters.unit = MILLIMETER; // this sample is designed to work in METER
    parameters.verbose = true;
    ERRCODE err = zed->init(parameters);
    // ERRCODE display
    cout << errcode2str(err) << endl;

    // Quit if an error occurred
    if (err != SUCCESS) {
        delete zed;
        return 1;
    }

    // get width and height of the ZED images
    int width = zed->getImageSize().width;
    int height = zed->getImageSize().height;

    sl::zed::SENSING_MODE dm_type = sl::zed::STANDARD;

     //the depth is limited to 20. METERS as define in zed::init()
    zed->setDepthClampValue(20000);
    Mat depth;
#ifdef MOUSE3D
    Mat depthGPU;
    depthGPU.data = (unsigned char*) nppiMalloc_32f_C4(width, height, &depth.step);
    depthGPU.setUp(width, height, 4, sl::zed::FLOAT, GPU);

    mouse3D Mouse(&depth, cv::Size(width, height), parameters.unit, "Depth");
    sl::zed::MEASURE mean = sl::zed::MEASURE::XYZRGBA;
    cv::Mat imageDisplay(height, width, CV_32FC4);

#else
    Mat depthGPU;
    depthGPU.data = (unsigned char*) nppiMalloc_32f_C1(width, height, &depth.step);
    depthGPU.setUp(width, height, 1, sl::zed::FLOAT, GPU);

    mouseDistance Mouse(&depth, cv::Size(width, height), parameters.unit, "Depth");
    sl::zed::MEASURE mean = sl::zed::MEASURE::DEPTH;
    cv::Mat imageDisplay(height, width, CV_32FC1);
#endif

    char key = ' ';
    // launch a loop
    bool run = true;
    while (run) {

        ERRCODE res = zed->grab(dm_type,1,1,1);
        //cout << errcode2str(res)  << endl;
        if (res == SUCCESS) {
             depthGPU = zed->retrieveMeasure_gpu(mean);
             //Mouse.updateData(&depth);
         //Copy the processed image frome the GPU to the CPU for display
        cudaMemcpy2D((uchar*) imageDisplay.data, imageDisplay.step, (Npp8u*) depthGPU.data, depthGPU.step, depthGPU.getWidthByte(), depthGPU.height, cudaMemcpyDeviceToHost);
        }
        depth=cvMat2slMat(imageDisplay);
         Mouse.updateData(&depth);
        //cv::vector<cv::Point3f> dist = Mouse.getdist();
        //for(size_t i=0; i<dist.size(); i++)
            //cv::putText(imageDisplay, to_string(dist[i].z), cv::Point((int)dist[i].x, (int)dist[i].y), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 255, 255), 2);
        // display the result
        cv::imshow("Depth", imageDisplay);
        //cv::imshow("View",imageDisplayDepth);
        key = cv::waitKey(20);

        switch (key)// handle the pressed key
        {
            case 'q': // close the program
            case 'Q':
                run = false;
                break;
            case 's':
            case 'S':
                imwrite("./1.jpg", imageDisplay);
                //imwrite("./2.jpg",imageDisplayDepth);
                break;
            default:
                break;
        }
    }

    // free all the allocated memory before quit
    imageDisplay.release();
    depthGPU.deallocate();
    delete zed;

    return 0;
}


